#include "DrawLine.h"

void DrawArrow(const btVector3& start, const btVector3& end) {
	DrawArrow(start.x(), start.y(), start.z(), end.x(), end.y(), end.z(), 1.0, 0.0, 0.0);
}

void DrawArrow(const btVector3& start, const btVector3& end, const btVector3& color) {
	DrawArrow(start.x(), start.y(), start.z(), end.x(), end.y(), end.z(), color.x(), color.y(), color.z());
}

void DrawArrow(float x1, float y1, float z1, float x2, float y2, float z2, float r, float g, float b)
{
	glPushMatrix();
	glPushAttrib( GL_POLYGON_BIT ); // includes GL_CULL_FACE
	glDisable(GL_CULL_FACE); // draw from all sides

	float v[3];
	// Calculate arrow parameters:d
	v[0] = x2-x1;
	v[1] = y2-y1;
	v[2] = z2-z1;

	glColor3f(r, g, b);

	// Draw single line:
	glBegin(GL_LINES);
	glVertex3f(x1, y1, z1); // from
	glVertex3f(x2, y2, z2); // to
	glEnd(); // GL_LINES

	// Easy enough - now let's make the cone in the arrow!
	if (1 == 0) // Make a cone! Change test to "0 == 1" to disable this part...
	{
		float norm_of_v = sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );

		// Size of cone in arrow:
		float coneFractionAxially = 0.025;
		float coneFractionRadially = 0.12;

		float coneHgt = coneFractionAxially * norm_of_v;
		float coneRadius = coneFractionRadially * norm_of_v;

		float normalized_v[3];
		normalized_v[0] = v[0] / norm_of_v;
		normalized_v[1] = v[1] / norm_of_v;
		normalized_v[2] = v[2] / norm_of_v;

		// Construct transformation matrix
		float mat44[16] =
		{1,0,0,0,
		0,1,0,0,
		0,0,1,0,
		0,0,0,1};

		mat44[0] = normalized_v[0]; // [0,0]
		mat44[5] = normalized_v[1]; // [1,1]
		mat44[10] = normalized_v[2]; // [2,2]
		mat44[15] = 1.0;

		float vConeLocation[3];
		//vConeLocation[0] = (1.0-coneFractionAxially) * v[0];
		//vConeLocation[1] = (1.0-coneFractionAxially) * v[1];
		//vConeLocation[2] = (1.0-coneFractionAxially) * v[2];
		vConeLocation[0] = x1;
        vConeLocation[1] = y1;
        vConeLocation[2] = z1;

		// Move and rotate in position:
		glTranslatef( vConeLocation[0], vConeLocation[1], vConeLocation[2] );
		if (0 == 1) //  <===== PROBLEM HERE?!?! WHAT?
		{
			//glLoadIdentity()
			glMultMatrixf( mat44 );
		}
		GLUquadric* cone_obj = gluNewQuadric();
		gluCylinder(cone_obj, 0, coneHgt, coneRadius, 8, 1);
	}
	glPopAttrib(); // GL_CULL_FACE
	glPopMatrix();
}

void DrawBox(btTransform& tf, btVector3& size, btVector3& color) {

	glPushMatrix();
	btVector3 org = tf.getOrigin();
	glTranslatef(org.x(), org.y(), org.z());

	btVector3 halfExtent = size;

	static int indices[36] = {
		0,1,2,
		3,2,1,
		4,0,6,
		6,0,2,
		5,1,4,
		4,1,0,
		7,3,1,
		7,1,5,
		5,4,7,
		7,4,6,
		7,2,3,
		7,6,2};

	btVector3 vertices[8]={	
		btVector3(halfExtent[0],halfExtent[1],halfExtent[2]),
		btVector3(-halfExtent[0],halfExtent[1],halfExtent[2]),
		btVector3(halfExtent[0],-halfExtent[1],halfExtent[2]),	
		btVector3(-halfExtent[0],-halfExtent[1],halfExtent[2]),	
		btVector3(halfExtent[0],halfExtent[1],-halfExtent[2]),
		btVector3(-halfExtent[0],halfExtent[1],-halfExtent[2]),	
		btVector3(halfExtent[0],-halfExtent[1],-halfExtent[2]),	
		btVector3(-halfExtent[0],-halfExtent[1],-halfExtent[2])};

	glColor3f(color.x(), color.y(), color.z());
	glEnable(GL_LIGHTING);
	glBegin (GL_TRIANGLES);
	int si=36;
	for (int i=0;i<si;i+=3)
	{
		const btVector3& v1 = vertices[indices[i]];;
		const btVector3& v2 = vertices[indices[i+1]];
		const btVector3& v3 = vertices[indices[i+2]];
		btVector3 normal = (v3-v1).cross(v2-v1);
		normal.normalize ();
		glNormal3f(normal.getX(),normal.getY(),normal.getZ());
		glVertex3f (v1.x(), v1.y(), v1.z());
		glVertex3f (v2.x(), v2.y(), v2.z());
		glVertex3f (v3.x(), v3.y(), v3.z());

	}
	glEnd();


	glPopMatrix();
}